/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <string>
#include "dal/allocator.h"
#include "dal/service.h"
#include "dal_fastdds/utils/log.hpp"
#include "dal_fastdds/utils/EntityGetter.hpp"
#include "dal_fastdds/utils/ServiceRequest.hpp"
#include "dal_fastdds/converter/QosConverter.hpp"
#include "dal_fastdds/entity_impl/DalServiceImpl.hpp"
#include "dal_fastdds/config_manager/ConfigManager.hpp"
#include "dal_utils/MsgLibLoader.hpp"

using namespace eprosima::fastdds::dds;
using namespace dal_fastdds;

static dal_service_t* CreateDalServiceByQos(dal_domain_participant_t* participant,
                                            const char* service_name,
                                            const char* request_reader_type_name,
                                            const char* response_writer_type_name,
                                            DataReaderQos& f_rqos,
                                            DataWriterQos& f_wqos)
{
    DomainParticipant* f_participant = get_fastdds_domain_participant(participant);
    if (!f_participant) {
        LOG_ERROR_WITH_LOCATION() << "get fastdds domain participant failed." << LOG_ENDL;
        return nullptr;
    }

    const auto& dal_part_impl = reinterpret_cast<DalDomainParticipantImpl*>(participant->handle_);
    if (!dal_part_impl->CreateCSPubSub()) {
        LOG_ERROR_WITH_LOCATION() << "CreateCSPubSub failed." << LOG_ENDL;
        return nullptr;
    }

    // get type support
    auto req_reader_ts = static_cast<const message_type_support_callbacks_t*>(
        MsgLibLoader::typeSupportMap_[request_reader_type_name]);
    auto resp_writer_ts = static_cast<const message_type_support_callbacks_t*>(
        MsgLibLoader::typeSupportMap_[response_writer_type_name]);
    if (!req_reader_ts || !resp_writer_ts) {
        LOG_ERROR_WITH_LOCATION() << "req reader or resp writer type support not found."
                                  << LOG_ENDL;
        return nullptr;
    }

    // find or create topics
    DalServiceImpl* service_impl = new DalServiceImpl(service_name, req_reader_ts, resp_writer_ts);
    service_impl->SetParticipant(f_participant);
    service_impl->SetCSPublisher(dal_part_impl->CSPublisher());
    service_impl->SetCSSubscriber(dal_part_impl->CSSubscriber());

    TypeSupport req_type;
    TopicDescription* req_topic_desc = nullptr;
    if (!service_impl->GetFastddsReqType(req_topic_desc, req_type)) {
        LOG_ERROR_WITH_LOCATION() << "get request type failed." << LOG_ENDL;
        return nullptr;
    }

    TypeSupport resp_type;
    TopicDescription* resp_topic_desc = nullptr;
    if (!service_impl->GetFastddsRespType(resp_topic_desc, resp_type)) {
        LOG_ERROR_WITH_LOCATION() << "get response type failed." << LOG_ENDL;
        return nullptr;
    }

    if (ReturnCode_t::RETCODE_OK != req_type.register_type(f_participant)) {
        LOG_ERROR_WITH_LOCATION() << "register req type failed." << LOG_ENDL;
        return nullptr;
    }
    if (ReturnCode_t::RETCODE_OK != resp_type.register_type(f_participant)) {
        LOG_ERROR_WITH_LOCATION() << "register resp type failed." << LOG_ENDL;
        return nullptr;
    }
    service_impl->req_fastdds_type_ = req_type;
    service_impl->resp_fastdds_type_ = resp_type;

    if (!service_impl->GetFastddsReqTopic(req_topic_desc) ||
        !service_impl->GetFastddsRespTopic(resp_topic_desc)) {
        LOG_ERROR_WITH_LOCATION() << "get req or resp type failed." << LOG_ENDL;
        return nullptr;
    }

    dal_part_impl->AddCSTopic(dynamic_cast<Topic*>(req_topic_desc));
    dal_part_impl->AddCSTopic(dynamic_cast<Topic*>(resp_topic_desc));


    // create listeners
    service_impl->req_reader_listener_ = new ServiceReqReaderListenerImpl(service_impl);
    if (!service_impl->req_reader_listener_) {
        LOG_ERROR_WITH_LOCATION() << "create req reader listener failed." << LOG_ENDL;
        return nullptr;
    }

    service_impl->resp_writer_listener_ = new ServiceRespWriterListenerImpl(service_impl);
    if (!service_impl->resp_writer_listener_) {
        LOG_ERROR_WITH_LOCATION() << "create resp writer listener failed." << LOG_ENDL;
        return nullptr;
    }

    // create req_reader
    //! TODO: listener
    DataReader* req_reader = dal_part_impl->CSSubscriber()->create_datareader(
        req_topic_desc, f_rqos, nullptr, StatusMask::none());
    if (!req_reader) {
        LOG_ERROR_WITH_LOCATION() << "create request reader failed." << LOG_ENDL;
        return nullptr;
    }

    // create resp_writer
    //! TODO: listener
    DataWriter* resp_writer = dal_part_impl->CSPublisher()->create_datawriter(
        dynamic_cast<Topic*>(resp_topic_desc), f_wqos, nullptr, StatusMask::none());
    if (!resp_writer) {
        LOG_ERROR_WITH_LOCATION() << "create response writer failed." << LOG_ENDL;
        return nullptr;
    }

    service_impl->SetReqReader(req_reader);
    service_impl->SetRespWriter(resp_writer);

    // create dal_service
    dal_service_t* d_service = dal_service_allocate();
    if (!d_service) {
        LOG_ERROR_WITH_LOCATION() << "dal_service_allocate failed." << LOG_ENDL;
        return nullptr;
    }
    // d_service->req_reader_type_support_impl_ = req_reader_ts;
    // d_service->resp_writer_type_support_impl_ = resp_writer_ts;
    d_service->handle_ = service_impl;

    return d_service;
}

dal_service_t* dal_domain_participant_create_service(dal_domain_participant_t* participant,
                                                     const char* service_name,
                                                     const char* request_reader_type_name,
                                                     const char* response_writer_type_name,
                                                     dal_datareader_qos_t* request_reader_qos,
                                                     dal_datawriter_qos_t* response_writer_qos)
{
    DataReaderQos f_rqos;
    QosConverter::GetInstance()->ConvertDataReaderQosFromDalToFastdds(request_reader_qos, &f_rqos);
    f_rqos.endpoint().history_memory_policy =
        eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_rqos.data_sharing().off();
#endif

    DataWriterQos f_wqos;
    QosConverter::GetInstance()->ConvertDataWriterQosFromDalToFastdds(response_writer_qos, &f_wqos);
    f_wqos.endpoint().history_memory_policy =
        eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;

#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_wqos.data_sharing().off();
#endif

    return CreateDalServiceByQos(participant,
                                 service_name,
                                 request_reader_type_name,
                                 response_writer_type_name,
                                 f_rqos,
                                 f_wqos);
}

dal_service_t*
dal_domain_participant_create_service_with_profile(dal_domain_participant_t* participant,
                                                   const char* service_name,
                                                   const char* request_reader_type_name,
                                                   const char* response_writer_type_name,
                                                   dal_datareader_qos_t* request_reader_qos,
                                                   dal_datawriter_qos_t* response_writer_qos,
                                                   const char* request_reader_profile_name,
                                                   const char* response_writer_profile_name)
{
    DomainParticipant* f_participant = get_fastdds_domain_participant(participant);
    if (!f_participant) {
        LOG_ERROR_WITH_LOCATION() << "get fastdds domain participant failed." << LOG_ENDL;
        return nullptr;
    }

    const auto& dal_part_impl = reinterpret_cast<DalDomainParticipantImpl*>(participant->handle_);
    if (!dal_part_impl->CreateCSPubSub()) {
        LOG_ERROR_WITH_LOCATION() << "CreateCSPubSub failed." << LOG_ENDL;
        return nullptr;
    }

    DataReaderQos f_rqos;
    if (!request_reader_profile_name || strlen(request_reader_profile_name) == 0) {
        QosConverter::GetInstance()->ConvertDataReaderQosFromDalToFastdds(request_reader_qos,
                                                                          &f_rqos);
    } else {
        if (!ConfigManager::GetInstance()->getFastddsProfilePath().empty()) {
            try {
                dal_part_impl->CSSubscriber()->get_datareader_qos_from_profile(
                    request_reader_profile_name, f_rqos);
            }
            catch (...) {
                LOG_ERROR_WITH_LOCATION() << "get datareader qos from profile failed." << LOG_ENDL;
                QosConverter::GetInstance()->ConvertDataReaderQosFromDalToFastdds(
                    request_reader_qos, &f_rqos);
            }
        }
    }
    f_rqos.endpoint().history_memory_policy =
        eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;

#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_rqos.data_sharing().off();
#endif

    DataWriterQos f_wqos;
    if (!response_writer_profile_name || strlen(response_writer_profile_name) == 0) {
        QosConverter::GetInstance()->ConvertDataWriterQosFromDalToFastdds(response_writer_qos,
                                                                          &f_wqos);
    } else {
        if (!ConfigManager::GetInstance()->getFastddsProfilePath().empty()) {
            try {
                dal_part_impl->CSPublisher()->get_datawriter_qos_from_profile(
                    response_writer_profile_name, f_wqos);
            }
            catch (...) {
                LOG_ERROR_WITH_LOCATION() << "get datawriter qos from profile failed." << LOG_ENDL;
                QosConverter::GetInstance()->ConvertDataWriterQosFromDalToFastdds(
                    response_writer_qos, &f_wqos);
            }
        }
    }
    f_wqos.endpoint().history_memory_policy =
        eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;

#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_wqos.data_sharing().off();
#endif

    return CreateDalServiceByQos(participant,
                                 service_name,
                                 request_reader_type_name,
                                 response_writer_type_name,
                                 f_rqos,
                                 f_wqos);
}

dal_ret_t dal_domain_participant_delete_service(dal_domain_participant_t* participant,
                                                dal_service_t* service)
{
    DomainParticipant* f_participant = get_fastdds_domain_participant(participant);
    if (!f_participant) {
        LOG_ERROR_WITH_LOCATION() << "get fastdds domain participant failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    if (!service || !service->handle_) {
        LOG_ERROR_WITH_LOCATION() << "service is null." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }
    auto service_impl = static_cast<DalServiceImpl*>(service->handle_);
    if (!service_impl->Clear()) {
        LOG_ERROR_WITH_LOCATION() << "clear service failed." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }
    delete service_impl;
    dal_service_free(service);

    return DAL_RETCODE_OK;
}

dal_ret_t dal_service_take_request(dal_service_t* service,
                                   dal_service_info_t* request_header,
                                   void* request,
                                   bool* taken)
{
    if (!service || !service->handle_) {
        LOG_ERROR_WITH_LOCATION() << "service is null." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    if (!request_header) {
        LOG_ERROR_WITH_LOCATION() << "request_header is null." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    auto service_impl = static_cast<DalServiceImpl*>(service->handle_);

    ServiceRequest req;
    req.buffer_ = new eprosima::fastcdr::FastBuffer();

    if (!req.buffer_) {
        LOG_ERROR_WITH_LOCATION() << "new buffer failed." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    dal_fastrtps_shared_cpp::SerializedData sdata;
    sdata.is_cdr_buffer = true;
    sdata.data = req.buffer_;
    sdata.impl = nullptr;

    StackAllocatedSequence<void*, 1> data_values;
    const_cast<void**>(data_values.buffer())[0] = &sdata;
    SampleInfoSeq info_seq{1};

    if (ReturnCode_t::RETCODE_OK == service_impl->GetReqReader()->take(data_values, info_seq, 1)) {
        if (info_seq[0].valid_data) {
            req.sample_identity_ = info_seq[0].sample_identity;

            const GUID_t& client_resp_reader_guid =
                info_seq[0].related_sample_identity.writer_guid();
            if (client_resp_reader_guid != GUID_t::unknown()) {
                req.sample_identity_.writer_guid() = client_resp_reader_guid;
            }

            const GUID_t& client_req_writer_guid = info_seq[0].sample_identity.writer_guid();
            service_impl->resp_writer_listener_->endpoint_add_reader_and_writer(
                client_resp_reader_guid, client_req_writer_guid);

            auto ts = dynamic_cast<dal_fastrtps_shared_cpp::TypeSupport*>(
                service_impl->req_fastdds_type_.get());
            eprosima::fastcdr::Cdr deser(*req.buffer_,
                                         eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
                                         eprosima::fastcdr::Cdr::DDS_CDR);
            if (ts->deserializeROSmessage(deser, request, service_impl->req_ts_)) {
                // set header
                dal_fastdds::copy_from_fastrtps_guid_to_byte_array(
                    req.sample_identity_.writer_guid(), request_header->request_id.writer_guid);
                request_header->request_id.sequence_number =
                    ((int64_t)req.sample_identity_.sequence_number().high) << 32 |
                    req.sample_identity_.sequence_number().low;
                request_header->source_timestamp = info_seq[0].source_timestamp.to_ns();
                request_header->received_timestamp = info_seq[0].source_timestamp.to_ns();

                *taken = true;
            }
        }
    }

    return DAL_RETCODE_OK;
}
dal_ret_t
dal_service_send_response(dal_service_t* service, dal_request_id_t* request_header, void* response)
{
    if (!service || !service->handle_) {
        LOG_ERROR_WITH_LOCATION() << "service is null." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    if (!request_header || !response) {
        LOG_ERROR_WITH_LOCATION() << "request_header or response is null." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    auto service_impl = static_cast<DalServiceImpl*>(service->handle_);

    eprosima::fastrtps::rtps::WriteParams wparams;
    dal_fastdds::copy_from_byte_array_to_fastrtps_guid(
        request_header->writer_guid, &wparams.related_sample_identity().writer_guid());
    wparams.related_sample_identity().sequence_number().high =
        (uint32_t)(request_header->sequence_number & 0xFFFFFFFF00000000 >> 32);
    wparams.related_sample_identity().sequence_number().low =
        (int32_t)(request_header->sequence_number & 0xFFFFFFFF);

    constexpr uint8_t entity_id_is_reader_bit = 0x04;
    const eprosima::fastrtps::rtps::GUID_t& related_guid =
        wparams.related_sample_identity().writer_guid();
    if (related_guid.entityId.value[3] & entity_id_is_reader_bit != 0) {
        // Related guid is a reader, so it is the response subscription guid.
        // Wait for the response writer to be matched with it.
        auto listener = service_impl->resp_writer_listener_;
        auto writer_max_blocking_time =
            service_impl->GetRespWriter()->get_qos().reliability().max_blocking_time;
        auto max_blocking_time = std::chrono::seconds(writer_max_blocking_time.seconds) +
                                 std::chrono::nanoseconds(writer_max_blocking_time.nanosec);
        ClientPresentStatus ret = listener->check_for_subscription(related_guid, max_blocking_time);
        if (ClientPresentStatus::GONE == ret) {
            return DAL_RETCODE_OK;
        } else if (ClientPresentStatus::MAYBE == ret) {
            LOG_ERROR_WITH_LOCATION() << "client will not receivce response." << LOG_ENDL;
            return DAL_RETCODE_TIMEOUT;
        }
    }

    dal_fastrtps_shared_cpp::SerializedData sdata;
    sdata.is_cdr_buffer = false;
    sdata.data = const_cast<void*>(response);
    sdata.impl = service_impl->resp_ts_;
    if (service_impl->GetRespWriter()->write(&sdata, wparams)) {
        return DAL_RETCODE_OK;
    }

    LOG_ERROR_WITH_LOCATION() << "send response failed." << LOG_ENDL;
    return DAL_RETCODE_ERROR;
}

void dal_service_set_on_request_callback(dal_service_t* service,
                                         dal_service_on_request_t on_request,
                                         const void* user_data)
{
    if (!service || !service->handle_) {
        LOG_ERROR_WITH_LOCATION() << "service is null." << LOG_ENDL;
        return;
    }

    auto service_impl = static_cast<DalServiceImpl*>(service->handle_);
    service_impl->req_reader_listener_->set_on_new_request_callback(on_request, user_data);
}